#include <AccelStepper.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1
#define DIR_PIN  D0
#define EN_PIN   D3

#define M0_PIN   D2
#define M1_PIN   D4
#define M2_PIN   D5

#define HALL_PIN D8

// ================== HALL THRESHOLDS ==================
// Adjust based on your sensor readings
#define ON_THRESHOLD  3800   // Magnet detected
#define OFF_THRESHOLD 3400   // Magnet released

// ================== TURNTABLE CONSTANTS ==================
#define MOTOR_STEPS   200
#define MICROSTEPS    16
#define GEAR_RATIO    5

// Total steps per full table rotation
#define STEPS_PER_REV (MOTOR_STEPS * MICROSTEPS * GEAR_RATIO)  // = 16000

// ================== SPEED SETTINGS ==================
#define SEARCH_SPEED    800   // Fast CW homing search
#define BACKOFF_SPEED   400   // CCW backoff
#define APPROACH_SPEED  200   // Slow precise approach

#define MOVE_MAX_SPEED  1200  // Normal motion speed
#define MOVE_ACCEL      600   // Acceleration for positioning

// ================== LOOKUP TABLE ==================
// Pre-calibrated absolute positions (in steps)
//
// IMPORTANT:
// - These values compensate for non-integer division (16000 / 6)
// - This avoids overlap issues like C1 ≈ C5
// - Motion is always CW, so values are arranged accordingly
//
// Indexing: LUT[compartment][home]
//
const long LUT[7][5] = {
  {0,      0,      0,      0,      0     }, // unused

  {0,      0,   4000,   8000,  12000    }, // C1
  {0,   2665,   6665,  10665,  14665    }, // C2
  {0,   5332,   9332,  13332,   1332    }, // C3
  {0,   7999,  11999,  15999,   3999    }, // C4
  {0,  10666,  14666,   2666,   6666    }, // C5
  {0,  13333,   1333,   5333,   9333    }, // C6
};

const char* COMP_LABEL[7] = {"", "C1","C2","C3","C4","C5","C6"};
const char* HOME_LABEL[5] = {"", "H1","H2","H3","H4"};

// ================== STEPPER ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL FILTER ==================
float filtered  = 0.0f;
bool  hallState = false;

// ================== STATE MACHINES ==================
enum HomingState { SEARCH, BACKOFF, APPROACH, HOMING_DONE };
enum SystemState { HOMING, POSITIONING, IDLE };

HomingState homingState = SEARCH;
SystemState sysState    = HOMING;

// ================== POSITION TRACKING ==================
long turntableStepPos = 0;  // absolute position (0–15999)
int  lastComp         = 0;
int  lastHome         = 0;

// ================== HALL SENSOR ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);

  // Low-pass filter (noise smoothing)
  filtered = 0.8f * filtered + 0.2f * (float)raw;

  // Hysteresis (prevents flicker)
  if (!hallState && filtered > ON_THRESHOLD)  hallState = true;
  if ( hallState && filtered < OFF_THRESHOLD) hallState = false;

  return hallState;
}

// ================== HOMING LOGIC ==================
void runHoming() {

  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (homingState) {

    // ---- STEP 1: FAST CW SEARCH ----
    case SEARCH:
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {
        Serial.println("[HOMING] Magnet detected → backoff");
        homingState = BACKOFF;
      }
      break;

    // ---- STEP 2: BACKOFF CCW ----
    case BACKOFF:
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();

      if (!currentHall && lastHall) {
        Serial.println("[HOMING] Magnet cleared → approach");
        homingState = APPROACH;
      }
      break;

    // ---- STEP 3: SLOW CW APPROACH ----
    case APPROACH:
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {

        // Stop motion
        stepper.setSpeed(0);

        // Define absolute zero
        // This enforces: C1 = H1
        stepper.setCurrentPosition(0);
        turntableStepPos = 0;

        lastComp = 1;
        lastHome = 1;

        // Switch to normal motion mode
        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        stepper.setAcceleration(MOVE_ACCEL);

        homingState = HOMING_DONE;
        sysState    = IDLE;

        Serial.println("HOMING COMPLETE → C1 aligned with H1");
        printHelp();
      }
      break;

    case HOMING_DONE:
      break;
  }

  lastHall = currentHall;
}

// ================== MOVE FUNCTION (CW ONLY) ==================
void moveCompartmentToHome(int c, int h) {

  if (c < 1 || c > 6) { Serial.println("[ERR] C must be 1–6"); return; }
  if (h < 1 || h > 4) { Serial.println("[ERR] H must be 1–4"); return; }

  long target = LUT[c][h];

  // Compute CW-only movement
  long delta = target - turntableStepPos;
  if (delta < 0) delta += STEPS_PER_REV;

  if (delta == 0) {
    Serial.println("[INFO] Already at target");
    return;
  }

  // Update internal tracking
  turntableStepPos = target;
  lastComp = c;
  lastHome = h;

  // Command motion
  stepper.moveTo(stepper.currentPosition() + delta);
  sysState = POSITIONING;

  Serial.print("Moving ");
  Serial.print(COMP_LABEL[c]);
  Serial.print(" → ");
  Serial.println(HOME_LABEL[h]);
}

// ================== SERIAL COMMAND PARSER ==================
void parseCommand(String cmd) {

  cmd.trim();
  cmd.toUpperCase();

  // Format: CnHm (example: C3H2)
  if (cmd.length() == 4 && cmd[0]=='C' && cmd[2]=='H') {
    int c = cmd[1] - '0';
    int h = cmd[3] - '0';
    moveCompartmentToHome(c, h);
    return;
  }

  if (cmd == "HOME") {
    homingState = SEARCH;
    sysState    = HOMING;
    Serial.println("[HOMING] Restarting...");
    return;
  }

  if (cmd == "POS") {
    Serial.print("Steps: ");
    Serial.println(turntableStepPos);
    return;
  }

  if (cmd == "HELP") {
    printHelp();
    return;
  }

  Serial.println("[ERR] Unknown command");
}

// ================== HELP ==================
void printHelp() {
  Serial.println("\nCommands:");
  Serial.println("  CnHm  → Move (C1–C6, H1–H4)");
  Serial.println("  HOME  → Re-home");
  Serial.println("  POS   → Show position");
  Serial.println("  HELP  → Show commands\n");
}

// ================== SETUP ==================
void setup() {
  Serial.begin(115200);

  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  // DRV8825 → 1/16 microstepping
  pinMode(M0_PIN, OUTPUT); digitalWrite(M0_PIN, LOW);
  pinMode(M1_PIN, OUTPUT); digitalWrite(M1_PIN, LOW);
  pinMode(M2_PIN, OUTPUT); digitalWrite(M2_PIN, HIGH);

  filtered = analogRead(HALL_PIN);

  stepper.setMaxSpeed(SEARCH_SPEED);

  Serial.println("Starting homing...");
}

// ================== LOOP ==================
void loop() {

  switch (sysState) {

    case HOMING:
      runHoming();
      break;

    case POSITIONING:
      if (stepper.distanceToGo() != 0) {
        stepper.run();
      } else {
        Serial.println("Move complete");
        sysState = IDLE;
      }
      break;

    case IDLE:
      if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        parseCommand(cmd);
      }
      break;
  }
}